#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <Eigen/Dense>
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <nav_msgs/msg/odometry.hpp>
class NavsatTransformer: public rclcpp::Node
{
public:
    explicit NavsatTransformer(const rclcpp::NodeOptions &);
    ~NavsatTransformer();
private:
    std::string gps_frame_id_;
    bool gps_updated_;
    bool has_transform_gps_,transform_good_,use_local_cartesian_,use_manual_datum_;
    bool has_read_imu_ori;
    double utm_meridian_convergence_;
    std::string utm_zone_;
    rclcpp::Time gps_update_time_;
    tf2::Transform latest_cartesian_pose_;
    tf2::Transform transform_cartesian_pose_;
    double transform_cartesian_pose_timestamp_;
    Eigen::MatrixXd latest_cartesian_covariance_;
    GeographicLib::LocalCartesian gps_local_cartesian_;
    geometry_msgs::msg::Quaternion gps_orientation;
    
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr utm_odom_pub_;
    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_sub_;
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
    
    void gpsFixCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg);
    void setTransformGps(const sensor_msgs::msg::NavSatFix::SharedPtr & msg);
    void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg);
};
